/*
 * spl06.c
 *
 *  Created on: Sep 13, 2021
 *      Author: soliber
 */
#include "spl06.h"
#include "cat1.h"
#include <stdio.h>
#include <stdlib.h>
#include "cmsis_os.h"
#include "saveconfig.h"

extern osThreadId presTaskHandle;

const float scale_factor[]={
		524288.0f,1572864.0f,
		3670016.0f,7864320.0f,
		253952.0f,516096.0f,
		1040384.0f,2088960.0f
};

static struct{

	//info
	uint8_t addr;
	uint8_t id;

	//config
	spl_op_mode_t mode;
	spl_measure_cfg_t temp_cfg;
	spl_measure_cfg_t pres_cfg;

	//coefficients
	struct {
		int16_t c0,c1;
		int32_t c00,c10;
		int16_t c01,c11,c20,c21,c30;
	} coe;
	float kT,kP;

	//result
	int32_t fifo[32];
	double pres;
	double comp;

} _spl;


/* IO Port */

HAL_StatusTypeDef spl_write(uint8_t reg,uint8_t *data,uint8_t len)
{
	return HAL_I2C_Mem_Write(&hi2c1, _spl.addr<<1, reg, I2C_MEMADD_SIZE_8BIT, data, len, 10);
}

HAL_StatusTypeDef spl_read(uint8_t reg,uint8_t *data,uint8_t len)
{
	return HAL_I2C_Mem_Read(&hi2c1, _spl.addr<<1, reg, I2C_MEMADD_SIZE_8BIT, data, len, 10);
}

HAL_StatusTypeDef spl_write_byte(uint8_t reg,uint8_t data)
{
	return spl_write(reg,&data,1);
}

uint8_t spl_read_byte(uint8_t reg)
{
	uint8_t data;
	spl_read(reg,&data,1);
	return data;
}

/* others */
uint8_t spl_get_id(void)
{
	return spl_read_byte(SPL_ID);
}

void spl_get_coef(void)
{
	uint8_t buf[SPL_COEF_LEN];
	spl_read(SPL_COEF, buf, sizeof(buf));


	/**
	 * @note binary complement numbers
	 */
    _spl.coe.c0 = (uint16_t)buf[0] << 4 | (uint16_t)buf[1] >> 4;
    _spl.coe.c0 = (_spl.coe.c0 & 1 << 11) ? (0xf000 | _spl.coe.c0) : _spl.coe.c0;

    _spl.coe.c1 = (uint16_t)(buf[1] & 0x0f) << 8 | (uint16_t)buf[2];
    _spl.coe.c1 = (_spl.coe.c1 & 1 << 11) ? (0xf000 | _spl.coe.c1) : _spl.coe.c1;

    _spl.coe.c00 = (uint32_t)buf[3] << 12 | (uint32_t)buf[4] << 4 | (uint16_t)buf[5] >> 4;
    _spl.coe.c00 = (_spl.coe.c00 & 1 << 19) ? (0xfff00000 | _spl.coe.c00) : _spl.coe.c00;

    _spl.coe.c10 = (uint32_t)(buf[5] & 0x0f) << 16 | (uint32_t)buf[6] << 8 | (uint32_t)buf[7];
    _spl.coe.c10 = (_spl.coe.c10 & 1 << 19) ? (0xfff00000 | _spl.coe.c10) : _spl.coe.c10;

    _spl.coe.c01 = (uint16_t)buf[8] << 8 | buf[9];
    _spl.coe.c11 = (uint16_t)buf[10] << 8 | buf[11];
    _spl.coe.c20 = (uint16_t)buf[12] << 8 | buf[13];
    _spl.coe.c21 = (uint16_t)buf[14] << 8 | buf[15];
    _spl.coe.c30 = (uint16_t)buf[16] << 8 | buf[17];

}

/* calculate */
float spl_get_comp_pres(int32_t pres_raw, int32_t temp_raw)
{
	float ftsc = (float)temp_raw / _spl.kT;
	float fpsc = (float)pres_raw / _spl.kP;
	float qua2 = (float)_spl.coe.c10 + fpsc * ((float)_spl.coe.c20 + fpsc * (float)_spl.coe.c30);
	float qua3 = ftsc * fpsc * ((float)_spl.coe.c11 + fpsc * (float)_spl.coe.c21);

	return (float)_spl.coe.c00 + fpsc * qua2 + ftsc * (float)_spl.coe.c01 + qua3;
}

float spl_get_comp_temp(int32_t temp_raw)
{
	float ftsc = (float)temp_raw/_spl.kT;
	return ((float)_spl.coe.c0*0.5 + (float)_spl.coe.c1*ftsc);
}

#ifdef PRESSURE_STATION
#include "rtc.h"
/**
 * @brief set rtc alarm and enter standby
 *
 * @param time standby time period in second
 */
void standby_wait_alarm(uint32_t time)
{
	RTC_AlarmTypeDef alarm;
	HAL_RTC_GetTime(&hrtc, &alarm.AlarmTime, RTC_FORMAT_BIN);
	alarm.Alarm = 1;
	alarm.AlarmTime.Seconds += time;
	HAL_RTC_SetAlarm(&hrtc, &alarm, RTC_FORMAT_BIN);

//	//清标志
//	if(__HAL_PWR_GET_FLAG(PWR_FLAG_SB)!=RESET)
//	{
//		__HAL_PWR_CLEAR_FLAG(PWR_FLAG_SB);
//	}
//
//	//唤醒键失能
//	HAL_PWR_DisableWakeUpPin(PWR_WAKEUP_PIN1);
//	__HAL_PWR_CLEAR_FLAG(PWR_FLAG_WU);

//	HAL_PWR_EnterSTANDBYMode();
	HAL_PWR_EnterSTOPMode(PWR_LOWPOWERREGULATOR_ON, PWR_STOPENTRY_WFE);
	HAL_NVIC_SystemReset();
}
#include "cat1.h"
#endif

/**
 * initial spl06 as indoor navigation use case
 * @ref spl06 data sheet page 14
 */
HAL_StatusTypeDef spl_init(void)
{
	//wait for device ready
	while(!(spl_read_byte(SPL_MEAS_CFG)&(COEF_RDY_FLAG|SENSOR_RDY_FLAG)));

	//read coefficients
	spl_get_coef();

	//choose compensation scale factors
	_spl.kP = scale_factor[OVERSAMPLE_RATE_16HZ];
	_spl.kT = scale_factor[OVERSAMPLE_RATE_1HZ];

	//pressure: SampleRate 16Hz, OversamplingRate 16Hz
	spl_write_byte(SPL_PRS_CFG, SAMPLE_RATE_16HZ | OVERSAMPLE_RATE_16HZ);

	//temperature: SampleRate 1Hz, OversamplingRate 1Hz
	spl_write_byte(SPL_TMP_CFG, EXT_SENSOR | SAMPLE_RATE_1HZ | OVERSAMPLE_RATE_1HZ);

	//clear interrupt flags
	spl_read_byte(SPL_INT_STS);

	//FIFO Flush
	spl_write_byte(SPL_RESET,FIFO_FLUSH);

	//IRQ line active low
	//Generate interrupt when the FIFO is full
	//Enable FIFO
	//Enable P shift (because over sampling rate is > 8)
	spl_write_byte(SPL_CFG_REG,
			INT_ACTIVE_LOW|INT_FIFO_ENABLE|FIFO_ENABLE|PRES_BIT_SHIFT);

	//Background Mode: Continuous Pressure and Temperature
	spl_write_byte(SPL_MEAS_CFG,BACKGROUND_MODE_BOTH);

	/**
	 * above configuration makes pressure measurement time 27.6 ms, precision 1.2 PaRSM
	 * FIFO FULL interrupt period is about 2s (32 Samples / (16 Pressure Samples/s + 1 Temperature Samples/s))
	 */

	return HAL_OK;
}

/* Task */

void StartPresTask(void const * argument)
{
	HAL_StatusTypeDef st;
	float a = 1, b = 0;

#ifdef PRESSURE_STATION
	osDelay(10000);
#endif

	if(config_is_valid())
	{
		a = pconfig->cali.pres_a;
		b = pconfig->cali.pres_b;
		DEBUG_PRINT("load pres cali successed\r\n");
	}
	DEBUG_PRINT("pres cali: a=%.3f, b=%.3f\r\n",a,b);

	while(1)
	{
		//器件检测
		_spl.addr = SPL_ADDR_DEAFAULT;
		taskENTER_CRITICAL();
		st = HAL_I2C_IsDeviceReady(&hi2c1, _spl.addr<<1, 10, 10);
		taskEXIT_CRITICAL();
		if(st!=HAL_OK)
		{
			//检测不到对应的I2C设备，请检查设备地址或硬件线路！
			PRINT("SPL06 not found\r\n");
			Error_Handler();
		}

		//读取设备ID
		_spl.id = spl_get_id();

		//初始化（后台模式采集）
		spl_init();

		while(1)
		{
			//等待FIFO满信号
			osEvent evt;
			evt = osSignalWait(FIFO_FULL_INT_FLAG, osWaitForever);
			if(evt.status==osEventSignal)
			{
				uint8_t i = 0;
				int32_t pres_raw = 0,temp_raw = 0;
				uint8_t pres_cnt = 0,temp_cnt = 0;
				//读FIFO
				do
				{
					uint8_t buffer[3];
					int32_t raw_data;

					taskENTER_CRITICAL();
					spl_read(SPL_PSR_B2, buffer, 3);
					taskEXIT_CRITICAL();

					raw_data = (uint32_t)buffer[0] << 16 | (uint32_t)buffer[1] << 8 | (uint32_t)buffer[2];
					raw_data = (raw_data & 1 << 23) ? (0xff000000 | raw_data) : raw_data;

					//pressure data
					if(raw_data&1)
					{
						pres_raw += raw_data&0xfffffffe;
						pres_cnt ++;
					}
					//temperature data
					else
					{
						temp_raw += raw_data;
						temp_cnt ++;
					}
				}while(++i<32);

				//避免除零
				if((pres_cnt == 0) || (temp_cnt == 0))
					continue;

				//均值
				pres_raw /= pres_cnt;
				temp_raw /= temp_cnt;

				//计算并上报气压值
				float pres,temp;
				pres = a*spl_get_comp_pres(pres_raw,temp_raw)+b;
				temp = spl_get_comp_temp(temp_raw);
				DEBUG_PRINT("P:%3.2f,T:%3.2f\r\n",pres,temp);
				if(pres<0 || temp<0)
				{
					//数据异常，重新进行初始化
					break;
				}
				if(publish_enable)
				{
					char *p = malloc(64);
					if(p)
					{
						sprintf(p, "{\\\\22pressure\\\\22:%3.2f,\\\\22temperature\\\\22:%3.2f}",pres,temp);
						osMessagePut(publish_q_id, (uint32_t)p, 0);
					}
#ifdef PRESSURE_STATION
					static int n = 0;
					if(++n >= 3)
					{
						portENTER_CRITICAL();
						//4G模块关机
//						extern osThreadId cat1TaskHandle;
//						osSignalSet(cat1TaskHandle, CAT1_SIGNAL_POWERDOWN);
//						DEBUG_PRINT("aaa");
//						osSignalWait(CAT1_SIGNAL_POWERDOWN_OK, portMAX_DELAY);
						HAL_GPIO_WritePin(CAT1_EN_GPIO_Port,CAT1_EN_Pin, GPIO_PIN_SET);

						//待机10s
						standby_wait_alarm(1);
						portEXIT_CRITICAL();
					}
#endif
				}
			}
		}
	}
}

//spl中断处理
void spl_irq(void)
{
	//read and clear flags
	if(spl_read_byte(SPL_INT_STS)&FIFO_FULL_INT_FLAG)
	{
		osSignalSet(presTaskHandle, FIFO_FULL_INT_FLAG);
	}
	else
	{
		//不应该有其他中断，可能是噪声导致？
		PRINT("unexpected spl irq\r\n");
		Error_Handler();
	}
}

void spl_stop(void)
{
	spl_write_byte(SPL_MEAS_CFG, STANDBY_MODE);
}
